/*
******************************
* Copyright by Maxic Tech                                                      *
* All rights are reserved.
******************************
*/
#include <stdio.h>
#include <string.h>
#include "pico/stdlib.h"
#include "pico/binary_info.h"
#include "maxic_mt320x_als.h"

#define mt320x_debug_level 1
#define _DRV_TAG_ "maxic_mt320x_als.c"
#define mt320x_dbg(fmt, args...)                                             \
	do                                                                       \
	{                                                                        \
		if (mt320x_debug_level)                                              \
		{                                                                    \
			printf("mt320x: " _DRV_TAG_ " %d " fmt " \n", __LINE__, ##args); \
		}                                                                    \
	} while (0)
#define mt320x_info(fmt, args...)                                        \
	do                                                                   \
	{                                                                    \
		printf("mt320x: " _DRV_TAG_ " %d " fmt " \n", __LINE__, ##args); \
	} while (0)
#define mt320x_err(fmt, args...)                                                \
	do                                                                          \
	{                                                                           \
		printf("mt320x: error: " _DRV_TAG_ " %d " fmt " \n", __LINE__, ##args); \
	} while (0)

int32_t mt320x_set_als_time(struct mt320x_chip *mt320x_obj, uint16_t time)
{
	int ret = 0;
	mt320x_obj->params.als_time = ATIME_MS(time);
	maxic_i2c_write(MT320X_REG_ATIME, mt320x_obj->params.als_time);
	mt320x_dbg("mt320x_set_als_time() mt320x_obj->params.als_time:%d", mt320x_obj->params.als_time);

	return ret;
}

static void find_proper_gain_when_enable_als_sensor(struct mt320x_chip *mt320x_obj)
{
	mt320x_obj->find_proper_gain = true;
	mt320x_set_als_time(mt320x_obj, 3);	 // set atime to 2.78ms for choosing a proper gain quickly
	mt320x_set_als_gain(mt320x_obj, 16); // init gain 16x

	maxic_i2c_modify(MT320X_REG_ENABLE, AEN | PON, AEN | PON);
	mt320x_dbg("before usleep");
	sleep_us(4000);
	mt320x_dbg("after usleep");
	mt320x_read_als(mt320x_obj); // read als raw data

	// 2.78ms, saturation value is 2780*2=5560  5560 *80%= 4448 16x gain
	// while max value is 65535*80%/18(50ms) = 2,912
	//  2912/(128/16) = 364  128x gain
	if (mt320x_obj->als_inf.photopic_raw <= 364)
	{
		mt320x_dbg("find_proper_gain_when_enable_als_sensor() set gain 128x");
		mt320x_set_als_gain(mt320x_obj, 128);
	}
	else if (mt320x_obj->als_inf.photopic_raw > 2912)
	{
		mt320x_dbg("find_proper_gain_when_enable_als_sensor() set gain 2x");
		mt320x_set_als_gain(mt320x_obj, 2);
	}
	else
	{
		mt320x_dbg("find_proper_gain_when_enable_als_sensor() keep default gain 16x");
		// do nothing, keep 16x gain
	}

	maxic_i2c_modify(MT320X_REG_ENABLE, AEN, 0);
	mt320x_set_als_time(mt320x_obj, 50); // change back to 50ms after fix gain quickly
	maxic_i2c_modify(MT320X_REG_ENABLE, AEN, AEN);
}

int32_t mt320x_als_enable(struct mt320x_chip *mt320x_obj)
{
	int32_t ret = 0;

	mt320x_obj->als_is_ready = false;
	mt320x_obj->first_als = true;
	mt320x_obj->als_enabled = true;

	// mt320x_set_als_gain(mt320x_obj,16);//set default als gain 16x
	maxic_i2c_modify(MT320X_REG_ALS_PERS, APERS_MASK, ALS_PERSIST(0));

	maxic_i2c_write(MT320X_REG_ASTATUS, AINT); // clear AINT

	if (mt320x_obj->polling_mode == false)
	{
		maxic_i2c_modify(MT320X_REG_ALS_INTEN, AIEN, AIEN); // enalbe als interrupt
	}

	find_proper_gain_when_enable_als_sensor(mt320x_obj);

	mt320x_dbg("Open ALS successfully");

	return ret;
}

int32_t mt320x_read_als(struct mt320x_chip *mt320x_obj)
{
	int32_t ret;
	uint8_t als_data[6] = {0};

	ret = maxic_i2c_blk_read(MT320X_REG_ADATA_ALSL, &als_data[0], 4);
	ret = maxic_i2c_blk_read(MT320X_REG_ADATA_CL, &als_data[4], 2);

	mt320x_obj->als_inf.photopic_raw = (uint16_t)(als_data[1] << 8 | als_data[0]);
	mt320x_obj->als_inf.ir_raw = (uint16_t)(als_data[3] << 8 | als_data[2]);
	mt320x_obj->als_inf.clear_raw = (uint16_t)(als_data[5] << 8 | als_data[4]);

	mt320x_dbg(" photopic = %d, ir = %d, clear = %d, als_gain = %d",
			   mt320x_obj->als_inf.photopic_raw, mt320x_obj->als_inf.ir_raw, mt320x_obj->als_inf.clear_raw, mt320x_obj->params.als_gain);

	return ret;
}

int32_t mt320x_set_als_gain(struct mt320x_chip *mt320x_obj, uint16_t gain)
{
	int32_t ret1, ret2, ret3;
	uint8_t cfg1 = 0;
	uint8_t cfg2 = 0, cfg3 = 0;

	switch (gain)
	{
	case 25: // 0.25x
		cfg1 = AGAIN_0_25x;
		break;
	case 50: // 0.5x
		cfg1 = AGAIN_0_50x;
		break;
	case 1:
		cfg1 = AGAIN_1x;
		break;
	case 2:
		cfg1 = AGAIN_2x;
		break;
	case 4:
		cfg1 = AGAIN_4x;
		break;
	case 8:
		cfg1 = AGAIN_8x;
		break;
	case 16:
		cfg1 = AGAIN_16x;
		break;
	case 32:
		cfg1 = AGAIN_32x;
		break;
	case 64:
		cfg1 = AGAIN_64x;
		break;
	case 128:
		cfg1 = AGAIN_128x;
		break;
	case 256:
		cfg1 = AGAIN_256x;
		break;
	case 512:
		cfg1 = AGAIN_256x;
		cfg2 = ALS_GAIN_4x;
		break;
	case 1024:
		cfg1 = AGAIN_256x;
		cfg2 = ALS_GAIN_8x;
		break;

	default:
		break;
	}

	if ((gain != 512) || (gain != 1024))
	{
		cfg2 = ALS_GAIN_2x;
	}
	if ((gain == 64) || (gain == 128) || (gain == 256) || (gain == 512) || (gain == 1024))
	{
		cfg3 = ALS_CAP_CTRL_0;
	}
	else
	{
		cfg3 = ALS_CAP_CTRL_3;
	}

	ret1 = maxic_i2c_modify(MT320X_REG_GAIN_CTRL1, AGAIN_MSK, cfg1);
	ret2 = maxic_i2c_modify(MT320X_REG_GAIN_CTRL3, ALS_GAIN_MASK, cfg2);
	ret3 = maxic_i2c_modify(MT320X_REG_ANACFG3, ALS_CAP_CTRL_MSK, cfg3);

	if (mt320x_obj->als_enabled == true)
	{
		maxic_i2c_modify(MT320X_REG_ENABLE, AEN, 0);
		sleep_us(1000);
		maxic_i2c_modify(MT320X_REG_ENABLE, AEN, AEN);
	}

	if ((ret1 >= 0) && (ret2 >= 0) && (ret3 >= 0))
	{ // return value of 0 means success
		mt320x_obj->params.als_gain1 &= ~AGAIN_MSK;
		mt320x_obj->params.als_gain1 |= (cfg1 & AGAIN_MSK);

		mt320x_obj->params.als_gain2 &= ~ALS_GAIN_MASK;
		mt320x_obj->params.als_gain2 |= (cfg2 & ALS_GAIN_MASK);

		mt320x_obj->params.als_gain = gain;
	}
	mt320x_dbg("mt320x_set_als_gain() again:%d", mt320x_obj->params.als_gain);
	mt320x_dbg("mt320x_set_als_gain() mt320x_obj->params.als_gain1:%d", mt320x_obj->params.als_gain1 >> 4);
	mt320x_dbg("mt320x_set_als_gain() mt320x_obj->params.als_gain2:%d", mt320x_obj->params.als_gain2 >> 2);

	return ret1 || ret2 || ret3;
}

static void mt320x_calc_tgp(struct mt320x_chip *mt320x_obj)
{
	uint32_t sat, tgp; // time gain product;
	uint32_t tmp1, tmp2;

	uint32_t atime = mt320x_obj->params.als_time + 1;
	uint32_t als_gain2 = pow(2, (mt320x_obj->params.als_gain2 >> 2));

	/* Calculate the saturation level */
	tmp1 = MAX_ALS_VALUE;
	tmp2 = atime * 2780 * als_gain2;

	sat = tmp2 > tmp1 ? tmp1 : tmp2;
	sat = sat * 8 / 10;
	mt320x_dbg("calc_tgp() mt320x_obj->params.als_gain2:%d;als_gain2:%d,atime_reg: %d,tmp2 = %d",
			   mt320x_obj->params.als_gain2, als_gain2, atime, tmp2);

	/* Calculate product of ALS time and gain */
	atime *= ATIME_PER_STEP_X100;
	atime /= 100;

	tgp = mt320x_obj->params.als_gain * atime;

	mt320x_obj->als_inf.saturation = sat;
	mt320x_obj->als_inf.tgproduct = tgp;
	mt320x_dbg("calc_tgp() als_gain = %d, atime = %d ms, tgp = %d, sat = %d", mt320x_obj->params.als_gain, atime, tgp, sat);
}

int32_t mt320x_calculate_lux_formula(struct mt320x_chip *mt320x_obj)
{
	int32_t lux = 0;
	int32_t ir_photopic_ratio = 0;
	uint32_t tgp = 0;

	/* Calculate the IR ratio */
	ir_photopic_ratio = mt320x_obj->als_inf.ir_raw * 1000 / mt320x_obj->als_inf.photopic_raw;
	mt320x_dbg("ir_photopic_ratio*1000 = %d", ir_photopic_ratio);

	/* Lux equation */
	if (ir_photopic_ratio < LUX_IR_DIV_POINT)
	{
		lux = A_COEF * mt320x_obj->als_inf.photopic_raw + B_COEF * mt320x_obj->als_inf.ir_raw; // low ir
	}
	else
	{
		lux = C_COEF * mt320x_obj->als_inf.photopic_raw + D_COEF * mt320x_obj->als_inf.ir_raw; // high ir
	}

	mt320x_dbg("step1 lux = %d", lux);

	if (lux < 0)
	{
		lux = 0;
	}

	// lux is signed 32 bit, max value is 4,294,967,296, forbid overflow

	tgp = mt320x_obj->als_inf.tgproduct;
	if (lux < (100 * tgp))
	{
		lux *= DGF;
		lux /= tgp;
	}
	else
	{
		lux /= tgp;
		lux *= DGF;
	}

	mt320x_dbg("step2 lux = %d, tgp = %d", lux, tgp);

	// lux += 500;

	/* Scalling */
	if (lux > 1000 * 1000)
	{
		lux /= 1000;
		lux *= mt320x_obj->als_inf.als_factor;
	}
	else
	{
		lux *= mt320x_obj->als_inf.als_factor;
		lux /= 1000;
	}

	mt320x_dbg("step3 lux = %d, mt320x_obj->als_inf.als_factor = %d", lux, mt320x_obj->als_inf.als_factor);

	lux /= (1000 * 1000); // for DGF / A B C D coef has been muliplied by 1000

	if (lux > MAX_ALS_VALUE)
	{
		lux = MAX_ALS_VALUE;
	}

	return lux;
}

int32_t mt320x_get_lux(struct mt320x_chip *mt320x_obj)
{
	int32_t ret = -1;
	int32_t lux = 0;
	uint32_t sat;
	uint16_t gain = mt320x_obj->params.als_gain;

	/* Use time in ms and get scaling factor */
	mt320x_calc_tgp(mt320x_obj);
	sat = mt320x_obj->als_inf.saturation;

	if (!mt320x_obj->als_gain_auto)
	{
		mt320x_obj->als_is_ready = true;
		if (mt320x_obj->als_inf.photopic_raw <= MIN_ALS_VALUE)
		{
			lux = 0;
			goto exit;
		}
		else if (mt320x_obj->als_inf.photopic_raw >= sat)
		{
			lux = MAX_ALS_VALUE;
			goto exit;
		}
	}
	else
	{
		// mt320x_dbg("mt320x_get_lux: gain = %d", gain);
		/* Auto ALS gain setting is enabled */
		if (gain == 16 && mt320x_obj->als_inf.photopic_raw >= sat)
		{
			mt320x_dbg("gain switch, current gain = %d, change gain to 2", gain);
			ret = mt320x_set_als_gain(mt320x_obj, 2);
		}
		else if (gain == 16 && mt320x_obj->als_inf.photopic_raw < GAIN_SWITCH_LEVEL)
		{
			mt320x_dbg("gain switch,  current gain = %d, change gain to 128", gain);
			ret = mt320x_set_als_gain(mt320x_obj, 128);
		}
		else if ((gain == 128 && mt320x_obj->als_inf.photopic_raw >= sat) || (gain == 2 && mt320x_obj->als_inf.photopic_raw < GAIN_SWITCH_LEVEL))
		{
			mt320x_dbg("gain switch,  current gain = %d, change gain to 16", gain);
			ret = mt320x_set_als_gain(mt320x_obj, 16); // ret value of 0 means gain set successfully
		}
		if (!ret)
		{
			/* ALS gain is changed, give up current lux calculation */
			// mt320x_calc_tgp(mt320x_obj);
			lux = mt320x_obj->als_inf.lux;
			mt320x_obj->als_is_ready = false;
			goto exit;
		}

		/* ALS gain is stable, ALS data is available now */
		mt320x_obj->als_is_ready = true;

		if (mt320x_obj->als_inf.photopic_raw <= MIN_ALS_VALUE)
		{
			lux = 0;
			goto exit;
		}
		else if (mt320x_obj->als_inf.photopic_raw >= sat)
		{
			lux = MAX_ALS_VALUE;
			goto exit;
		}
	}

	/* Calculate Lux */
	lux = mt320x_calculate_lux_formula(mt320x_obj);

exit:
	mt320x_obj->als_inf.lux = (uint16_t)lux;

	mt320x_dbg("mt320x_get_lux:%d ", mt320x_obj->als_inf.lux);
	return 0;
}

static int32_t mt320x_get_avg_lux(struct mt320x_chip *mt320x_obj)
{
	uint32_t luxSum = 0;
	uint8_t i;
	int32_t ret = 0;
	uint32_t sleep_time = ATIME_PER_STEP_X100 * 10 * (mt320x_obj->params.als_time + 2); // unit is us

	mt320x_dbg("mt320x_obj->params.als_time:%d , sleep_time:%d us", mt320x_obj->params.als_time, sleep_time);

	for (i = 0; i < NUM_OF_LUX_TO_AVE;)
	{
		/* Wait ALS integration time before reading */
		sleep_us(sleep_time);

		mt320x_read_als(mt320x_obj);
		mt320x_get_lux(mt320x_obj);

		if (mt320x_obj->als_is_ready)
		{
			luxSum += mt320x_obj->als_inf.lux;
			i++;
			mt320x_dbg("mt320x_get_avg_lux i = %d ", i);
		}
	}

	mt320x_obj->als_inf.avg_lux = (luxSum / i);

	return ret;
}

int32_t mt320x_calibrate_als(struct mt320x_chip *mt320x_obj)
{
	int32_t ret = 0;
	uint32_t tgtLux, avgLux, tgtLuxLowRange, tgtLuxHiRange;
	uint8_t enable;
	avgLux = 1;
	tgtLux = 1;

	mt320x_obj->als_inf.als_factor = 1000; // set default value firstly

	/* Save ENABLE reg, and config the ALS sensor for calibration */
	maxic_i2c_read(MT320X_REG_ENABLE, &enable);
	maxic_i2c_modify(MT320X_REG_ENABLE, PON, PON);
	sleep_us(500);

	find_proper_gain_when_enable_als_sensor(mt320x_obj);

	/* Compare the average lux to the passed in calibration lux */
	mt320x_get_avg_lux(mt320x_obj);

	/* Resume ENABLE reg */
	maxic_i2c_write(MT320X_REG_ENABLE, enable);

	/* Find Lux mid way between calLuxLower and calLuxUpper */
	tgtLux = (CALIB_LUX_LOW + CALIB_LUX_HIGH) / 2;
	tgtLuxLowRange = tgtLux >> 1;
	tgtLuxHiRange = tgtLux + (tgtLux >> 1);
	avgLux = mt320x_obj->als_inf.avg_lux;

	mt320x_dbg("mt320x_calibrate_als: avg_lux:%d , tgtLux:%d ,tgtLuxLowRange :%d, tgtLuxHiRange:%d", avgLux, tgtLux, tgtLuxLowRange, tgtLuxHiRange);

	/* Check if avg lux is outside expected range of target lux. If so, return efail */
	// if (avgLux <= tgtLuxLowRange || avgLux >= tgtLuxHiRange){
	// 	mt320x_dbg("mt320x_calibrate_als failed");
	// 	return -1;
	// }

	mt320x_obj->als_inf.als_factor = (mt320x_obj->als_inf.als_factor * tgtLux) / avgLux;

	mt320x_dbg("mt320x_calibrate_als successfully: als_factor :%d", mt320x_obj->als_inf.als_factor);

	return ret;
}

void mt320x_update_als_thresh(struct mt320x_chip *mt320x_obj)
{
	uint8_t data[4];
	int32_t high_thresh = 0;
	int32_t low_thresh = 0;
	uint32_t delta = 0;
	uint32_t max_count = 0;
	uint8_t atime = mt320x_obj->params.als_time + 1;
	uint32_t als_gain2 = pow(2, (mt320x_obj->params.als_gain2 >> 2));
	int32_t tmp1, tmp2;

	static int32_t high_thresh_last = 0;
	static int32_t low_thresh_last = 0;

	memset((void *)data, 0, sizeof(data));

	tmp1 = MAX_ALS_VALUE;
	tmp2 = atime * 2780 * als_gain2;

	max_count = tmp2 > tmp1 ? tmp1 : tmp2;

	delta = mt320x_obj->als_inf.photopic_raw * 10 / 100; // 10 percent
	if (delta < ALS_DRI_MIN_DELTA)
	{
		delta = ALS_DRI_MIN_DELTA;
	}

	high_thresh = mt320x_obj->als_inf.photopic_raw + delta;
	low_thresh = mt320x_obj->als_inf.photopic_raw - delta;

	mt320x_dbg("mt320x_update_als_thresh tmp2 = %d, photopic_raw = %d, delta = %d, high_thresh :%d, low_thresh : %d", tmp2, mt320x_obj->als_inf.photopic_raw, delta, high_thresh, low_thresh);

	if (mt320x_obj->als_inf.photopic_raw >= high_thresh_last)
	{
		mt320x_dbg("als becomes bigger, photopic_raw = %d, high_thresh_last = %d, low_thresh_last = %d", mt320x_obj->als_inf.photopic_raw, high_thresh_last, low_thresh_last);
	}
	else if (mt320x_obj->als_inf.photopic_raw <= low_thresh_last)
	{
		mt320x_dbg("als becomes litter, photopic_raw = %d, high_thresh_last = %d, low_thresh_last = %d", mt320x_obj->als_inf.photopic_raw, high_thresh_last, low_thresh_last);
	}

	if (high_thresh > max_count)
	{
		high_thresh = max_count;
	}
	if (low_thresh < 0)
	{
		low_thresh = 0;
	}

	mt320x_obj->params.als_th_high = (uint16_t)high_thresh;
	mt320x_obj->params.als_th_low = (uint16_t)low_thresh;

	/* Write the threshold to registers */
	data[0] = (uint8_t)(mt320x_obj->params.als_th_low & 0xFF);
	data[1] = (uint8_t)((mt320x_obj->params.als_th_low >> 8) & 0xFF);
	data[2] = (uint8_t)(mt320x_obj->params.als_th_high & 0xFF);
	data[3] = (uint8_t)((mt320x_obj->params.als_th_high >> 8) & 0xFF);

	maxic_i2c_write(MT320X_REG_AILTL, data[0]);
	maxic_i2c_write(MT320X_REG_AILTH, data[1]);
	maxic_i2c_write(MT320X_REG_AIHTL, data[2]);
	maxic_i2c_write(MT320X_REG_AIHTH, data[3]);

	high_thresh_last = high_thresh;
	low_thresh_last = low_thresh;
}
